
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_airspeed.h
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed_backend.h"

#include <stdint.h>
#include <stdbool.h>

#include <common/gp_defines.h>
#include <vehicle/mb_fixed_wing.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum airspeed_type {
    AS_TYPE_NONE=0,
    AS_TYPE_I2C_MS4525=1,
    AS_TYPE_ANALOG=2,
    AS_TYPE_I2C_MS5525=3,
    AS_TYPE_I2C_MS5525_ADDRESS_1=4,
    AS_TYPE_I2C_MS5525_ADDRESS_2=5,
    AS_TYPE_I2C_SDP3X=6,
    AS_TYPE_I2C_DLVR_5IN=7,
    AS_TYPE_UAVCAN=8,
    AS_TYPE_I2C_DLVR_10IN=9,
    AS_TYPE_I2C_DLVR_20IN=10,
    AS_TYPE_I2C_DLVR_30IN=11,
    AS_TYPE_I2C_DLVR_60IN=12,
    AS_TYPE_NMEA_WATER=13,
    AS_TYPE_MSP=14,
    AS_TYPE_I2C_ASP5033=15,
    AS_TYPE_SITL=100,
};

enum AirspeedCalibrationState {
    AS_CAL_STATE_NOT_STARTED,
    AS_CAL_STATE_IN_PROGRESS,
    AS_CAL_STATE_SUCCESS,
    AS_CAL_STATE_FAILED
};

struct sensor_airspeed_params {
    // parameters for each instance
    Param_int32 bus_id;
    Param_float offset;
    Param_float ratio;
    Param_float psi_range;
    Param_int8  use;
    Param_int8  pin;
    Param_int8  skip_cal;
    Param_int8  tube_order;
    Param_int8  type;
    Param_int8  bus;
#if MB_AIRSPEED_AUTOCAL_ENABLE
    Param_int8  autocal;
#endif
};

typedef struct airspeed_calibration *airspeed_calibration_t;

struct airspeed_calibration {
    // state of kalman filter for airspeed ratio estimation
    matrix3f_t P; // covarience matrix
    float Q0; // process noise matrix top left and middle element
    float Q1; // process noise matrix bottom right element
    Vector3f_t state; // state vector
    float DT; // time delta
};

typedef struct sensor_airspeed *sensor_airspeed_t;
typedef const struct sensor_airspeed *sensor_airspeed_const_t;

struct sensor_airspeed {
    Param_int8 _enable;

    Param_int8 primary_sensor;
    Param_int8 max_speed_pcnt;
    Param_int32 _options;    // bitmask options for airspeed
    Param_float _wind_max;
    Param_float _wind_warn;
    Param_float _wind_gate;

    struct sensor_airspeed_params param[AIRSPEED_MAX_SENSORS];

    enum AirspeedCalibrationState calibration_state[AIRSPEED_MAX_SENSORS];

    struct airspeed_state {
        float   raw_airspeed;
        float   airspeed;
        float   last_pressure;
        float   filtered_pressure;
        float   corrected_pressure;
        uint64_t last_update_us;
        bool use_zero_offset;
        bool    healthy;

        // state of runtime calibration
        struct {
            uint32_t start_ms;
            float    sum;
            uint16_t count;
            uint16_t read_count;
        } cal;

#if MB_AIRSPEED_AUTOCAL_ENABLE
        struct airspeed_calibration calibration;
        float last_saved_ratio;
        uint8_t counter;
#endif // AP_AIRSPEED_AUTOCAL_ENABLE

        struct {
            uint32_t last_check_ms;
            float health_probability;
            float test_ratio;
            int8_t param_use_backup;
            uint32_t last_warn_ms;
        } failures;

#if MB_AIRSPEED_HYGROMETER_ENABLE
        uint32_t last_hygrometer_log_ms;
#endif
    } state[AIRSPEED_MAX_SENSORS];

    bool calibration_enabled;

    // can be set to true to disable the use of the airspeed sensor
    bool _force_disable_use;

    // current primary sensor
    uint8_t primary;
    uint8_t num_sensors;

    uint32_t _log_bit;     // stores which bit in LOG_BITMASK is used to indicate we should log airspeed readings

    sensor_as_backend_t sensor[AIRSPEED_MAX_SENSORS];

    mb_fixed_wing_const_t fixed_wing_parameters;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
sensor_airspeed_t sensor_as_get_singleton();

void sensor_as_ctor();
void sensor_as_init();
void sensor_as_allocate();
void sensor_as_read(uint8_t i);
void sensor_as_update();
bool sensor_as_lib_enabled();
bool sensor_as_enabled(void);
bool sensor_as_enabled2(uint8_t i);
float sensor_as_get_differential_pressure(uint8_t i);
void sensor_as_update_calibration(const Vector3f_t *vground, int16_t max_airspeed_allowed_during_cal);
void sensor_as_update_calibration2(uint8_t i, const Vector3f_t *vground, int16_t max_airspeed_allowed_during_cal);
void sensor_as_update_calibration3(uint8_t i, float raw_pressure);
void sensor_as_send_airspeed_calibration(const Vector3f_t *vground);
void sensor_as_assign_param();
float sensor_as_get_pressure(uint8_t i);
bool sensor_as_get_temperature(float *temperature);
bool sensor_as_get_temperature2(uint8_t i, float *temperature);
float sensor_as_get_airspeed(void);
float sensor_as_get_airspeed2(uint8_t i);
float sensor_as_get_raw_airspeed(void);
float sensor_as_get_raw_airspeed2(uint8_t i);
bool sensor_as_healthy(void);
bool sensor_as_healthy2(uint8_t i);
uint64_t sensor_as_last_update_us(void);
uint64_t sensor_as_last_update_us2(uint8_t i);
bool sensor_as_use();
bool sensor_as_use2(uint8_t i);
void sensor_as_check_sensor_failures();
void sensor_as_check_sensor_ahrs_wind_max_failures(uint8_t i);
void sensor_as_set_fixedwing_parameters(mb_fixed_wing_const_t _fixed_wing_parameters);

/// airspeed_calibration
void airspeed_calibration_ctor(airspeed_calibration_t airspeed_cal);
void airspeed_calibration_init(airspeed_calibration_t airspeed_cal, float initial_ratio);
float airspeed_calibration_update(airspeed_calibration_t airspeed_cal, float airspeed, const Vector3f_t *vg, int16_t max_airspeed_allowed_during_cal);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



